#include "pcqc.h"

Pcqc::Pcqc()
{
    targetCloud.reset (new PointCloud<PointXYZRGB>);
    sourceCloud.reset (new PointCloud<PointXYZRGB>);
    registeredCloud.reset (new PointCloud<PointXYZRGB>);
    differenceCloud.reset (new PointCloud<PointXYZRGB>);
    newComponentCloud.reset (new PointCloud<PointXYZRGB>);
    colThreshold = 0;
    cluThreshold = 0;
    skinThickness = 0;
    scoreThreshold = 2;
    distanceThreshold = 2;
    registrationAlreadyPerformed = false;
    newComponentPointIndices.reset (new PointIndices);
}

PointCloud<PointXYZRGB>::Ptr Pcqc::voxelCloud (PointCloud<PointXYZRGB>::Ptr input, double leafSize)
{
    //cout << "Voxelling... "<<flush; //DEBUG
    PointCloud<PointXYZRGB>::Ptr cloud_filtered (new PointCloud<PointXYZRGB>);
    VoxelGrid<PointXYZRGB> sor;
    sor.setInputCloud (input);
    sor.setLeafSize (leafSize,leafSize,leafSize);
    sor.filter (*cloud_filtered);
    return cloud_filtered;
}

void Pcqc::segmentation (PointCloud<PointXYZRGB>::Ptr source)
{
    //cout << "Plane segmentation..." <<flush; //DEBUG
    // fit plane and keep points above that plane
    ModelCoefficients::Ptr coefficients (new ModelCoefficients);
    PointCloud<PointXYZRGB>::Ptr segmented(new PointCloud<PointXYZRGB>);
    PointIndices::Ptr inliers (new PointIndices);
    SACSegmentation<PointXYZRGB> seg;
    seg.setOptimizeCoefficients (true);//default true
    seg.setModelType (SACMODEL_PERPENDICULAR_PLANE);//SACMODEL_PLANE
    seg.setMethodType (SAC_RANSAC);
    seg.setMaxIterations(50);//default 50
    seg.setAxis (Eigen::Vector3f (0.0, 0.0, 1.0));//parallelo asse z
    seg.setEpsAngle (deg2rad (5.0f));//15°tolleranza
    seg.setDistanceThreshold (5);
    seg.setInputCloud (source);
    seg.segment (*inliers, *coefficients);
    ExtractIndices<PointXYZRGB> extract;
    extract.setInputCloud (source);
    extract.setIndices (inliers);
    extract.setNegative (true);
    extract.filter (*segmented);
    *source=*segmented;
    segmented->clear();
    float zCut = abs(coefficients->values.at(3))-0.0f;
    for (int i=0;i<source->size();i++)  if(source->at(i).z < zCut) segmented->push_back(source->at(i));
    //cout << segmented->size();
    vector<int> indices;
    removeNaNFromPointCloud(*segmented, *segmented, indices);
    *source=*segmented;
}

void Pcqc::removeOutliers(PointCloud<PointXYZRGB>::Ptr cloud)
{
    //cout << "Removing outliers... " << flush; //DEBUG
    StatisticalOutlierRemoval<PointXYZRGB> sor;
    sor.setInputCloud (cloud);
    sor.setMeanK (2);//prima 20
    sor.setStddevMulThresh (1.0);//prima 1.0
    sor.filter (*cloud);

    vector<PointIndices> clusters =findClusters(cloud,1,500,cloud->size());
    float zCut=FLT_MAX;
    for(int i=0;i<clusters.size();i++)
        for(int j=0;j<clusters.at(i).indices.size();j++)
            if (cloud->at( clusters.at(i).indices.at(j)).z  < zCut) zCut=cloud->at( clusters.at(i).indices.at(j)).z;

    PointCloud<PointXYZRGB>::Ptr cleaned(new PointCloud<PointXYZRGB>);
    for(int i=0;i<cloud->size();i++)
        if(cloud->at(i).z>zCut)cleaned->push_back(cloud->at(i));
    //cout<<"filtered n. points: "<<cloud->size()-cleaned->size()<<endl;
    *cloud=*cleaned;
}

bool Pcqc::loadTargetCloud(QString path)
{
    registrationAlreadyPerformed = false;
    const std::string stdpath = path.toStdString();
    if(io::loadPCDFile(stdpath, *targetCloud) == 0 )
    {
        cout<<"\n###############################################"<<endl;
        cout<<"########################### LOADING TARGET FILE "<<path.toStdString()<<" ###################"<<endl;
        cout<<"###############################################"<<endl;
        vector<int> indices;
        removeNaNFromPointCloud(*targetCloud, *targetCloud, indices);
        targetCloud=voxelCloud(targetCloud,0.4);//0.4
        segmentation(targetCloud);
        removeOutliers(targetCloud);
        cout << "OK! " << targetCloud->size() << " points Loaded."<<endl;
        return true;
    }
    else return false;
}

bool Pcqc::loadSourceCloud(QString path)
{
    registrationAlreadyPerformed = false;
    const std::string stdpath = path.toStdString();
    if(io::loadPCDFile(stdpath, *sourceCloud) == 0)
    {
        cout<<"\n###############################################"<<endl;
        cout<<"######################## PROCESSING SOURCE FILE "<<path.toStdString()<<" ###################"<<endl;
        cout<<"###############################################"<<endl;
        vector<int> indices;
        removeNaNFromPointCloud(*sourceCloud, *sourceCloud, indices);
        sourceCloud = voxelCloud(sourceCloud,0.4); // alleggerisce il calcolo, da capire se peggiora il risultato o meno.
        segmentation(sourceCloud); // segmentazione del piano principale.
        //segmentation(sourceCloud);// segmentazione del piano PCB.
        removeOutliers(sourceCloud);
        cout << "OK! " << sourceCloud->size() << " points Loaded."<<endl;
        return true;
    }
    else return false;
}

//GETTERS
PointCloud<PointXYZRGB>::Ptr Pcqc::getTargetCloud()
{
    return targetCloud;
}

PointCloud<PointXYZRGB>::Ptr Pcqc::getSourceCloud()
{
    return sourceCloud;
}

PointCloud<PointXYZRGB>::Ptr Pcqc::getRegisteredCloud()
{
    return registeredCloud;
}

PointCloud<PointXYZRGB>::Ptr Pcqc::getDifferenceCloud()
{
    return differenceCloud;
}

QColor* Pcqc::getPointColor(int pointIndex)
{
    QColor *color = new QColor;
    color->setRed(targetCloud->at(pointIndex).r);
    color->setGreen(targetCloud->at(pointIndex).g);
    color->setBlue(targetCloud->at(pointIndex).b);
    return color;
}

PointCloud<PointXYZRGB>::Ptr Pcqc::getNewComponentCloud()
{
    return newComponentCloud;
}

PointCloud<PointXYZRGB>::Ptr Pcqc::getTargetComponentCloud(QString componentName)
{
    Component component = targetComponentsList.value(componentName);
    return component.getComponentCloud();
}

PointCloud<PointXYZRGB>::Ptr Pcqc::getSourceComponentCloud(QString componentName)
{
    Component component = sourceComponentsList.value(componentName);
    return component.getComponentCloud();
}

QMap<QString, Component>* Pcqc::getTargetComponentsList()
{
    return &targetComponentsList;
}

QMap<QString, Component>* Pcqc::getSourceComponentsList()
{
    return &sourceComponentsList;
}

QMap<QString, Check>* Pcqc::getChecksList()
{
    return &checksList;
}

bool Pcqc::isRegistrationAlreadyPerformed()
{
    return registrationAlreadyPerformed;
}

//SETTERS
void Pcqc::setClusterSegThreshold(int threshold)
{
    cluThreshold = (double)threshold/1000;
}

void Pcqc::setColorSegThreshold(int threshold)
{
    colThreshold = threshold;
}

void Pcqc::setShapeMatchingThickness(int thickness)
{
    skinThickness = (double)thickness/1000;
}

void Pcqc::setScoreDistanceThreshold(float scoreThresh, float distanceThresh)
{
    scoreThreshold = scoreThresh;
    distanceThreshold= distanceThresh;
}


//FUNCTIONS
void Pcqc::colorIndices(PointCloud<PointXYZRGB>::Ptr cloud, PointIndices::Ptr indices, int r, int g, int b)
{
    for (int i =0; i<indices->indices.size(); i++)
    {
        int currentPointIndex= indices->indices.at(i);
        cloud->at(currentPointIndex).r=r;
        cloud->at(currentPointIndex).g=g;
        cloud->at(currentPointIndex).b=b;
    }
}

void Pcqc::colorComponents(PointCloud<PointXYZRGB>::Ptr cloud, QMap<QString, Component> *components)
{
    QMapIterator<QString, Component> iter(*components);
    Component currentComponent;
    while (iter.hasNext())
    {
        currentComponent = iter.next().value();
        colorIndices(cloud, currentComponent.getIndices(), rand()%200, rand()%200, rand()%200); // use random color
    }
}

void Pcqc::componentSelection(int selectedPointIndex)
{
    cout << "Component Segmentation... "<<flush; // DEBUG PRINT

    lastClickedPointIndex = selectedPointIndex; // save the last clicked point index
    copyPointCloud(*targetCloud, *newComponentCloud); // start from a new copy of the cloud
    newComponentPointIndices->indices.clear();// and a new point indices

    PointIndices::Ptr tempClusterIndices(new PointIndices);
    vector<pcl::PointIndices> cluster_indices_out=findClusters(newComponentCloud,cluThreshold,1,newComponentCloud->size());
    segmentCluster(tempClusterIndices, cluster_indices_out, selectedPointIndex ); // CLUSTER SEGMENTATION

    PointIndices::Ptr tempColorIndices(new PointIndices);
    segmentColor(newComponentCloud, tempColorIndices, selectedPointIndex, colThreshold ); // COLOR SEGMENTATION

    intersectIndices(tempClusterIndices,tempColorIndices,newComponentPointIndices);

    colorIndices(newComponentCloud, newComponentPointIndices,0,255,0);

    cout << "OK! Selected "<< newComponentPointIndices->indices.size() <<" points for this component\n"<<flush; // DEBUG PRINT
}

bool Pcqc::componentSegmentation()
{
    copyPointCloud(*targetCloud, *newComponentPointIndices, *newComponentCloud);
    return true;
}

bool Pcqc::componentSave(QString componentName,bool isClusterMatching)
{
    if(targetComponentsList.find(componentName) == targetComponentsList.end())
    {
        PointIndices::Ptr indicesToSave(new PointIndices); // create new pointer to a new point indices
        indicesToSave->indices = newComponentPointIndices->indices; // fill the new indices with the new component's indices
        // create a new component and fill it with information
        if (isClusterMatching)
        {
        Component newComponentCluster(targetCloud, indicesToSave, lastClickedPointIndex, cluThreshold, colThreshold, skinThickness);
        targetComponentsList.insert(componentName, newComponentCluster); // instert the new component in the components list
        }
        else//is shape matching
        {
        Component newComponentShape(targetCloud, indicesToSave, -1, cluThreshold, colThreshold, skinThickness);
        targetComponentsList.insert(componentName, newComponentShape); // instert the new component in the components list
        }

        return true;
    }
    else return false;
}

bool Pcqc::componentDelete(QString componentName)
{
    return targetComponentsList.remove(componentName);
}

bool Pcqc::addCheck(QString checkName, QString componentName, CheckType type, float threshold)
{
    if(checksList.find(checkName) == checksList.end())
    {
        Check newCheck(componentName, type, threshold);
        checksList.insert(checkName, newCheck); // instert the new component in the components list
        return true;
    }
    else return false;
}

bool Pcqc::deleteCheck(QString checkName)
{
    return checksList.remove(checkName);
}

void Pcqc::registration()
{
    registerSourceToTarget(sourceCloud, targetCloud, registeredCloud);
    registrationAlreadyPerformed = true;
}

int Pcqc::findSourceComponents()
{
    QMapIterator<QString,Component> iter(targetComponentsList);
    Component componentToFind;
    Component componentFound;
    vector<pcl::PointIndices> cluster_indices_out=findClusters(registeredCloud,1,1,registeredCloud->size());
    while (iter.hasNext())
    {
        PointIndices::Ptr indicesFound (new PointIndices); // we need a new pointer every time, otherwise we would save only the last component

        componentToFind=iter.next().value();

        if (componentToFind.getGeneratingIndex() >= 0)//cluster matching
        componentMatchClickSimulation(registeredCloud,
                       targetCloud,
                       componentToFind,
                       indicesFound,
                       cluster_indices_out);
        else //shape matching
        componentMatchShapeCutting(registeredCloud,
                       componentToFind,
                       indicesFound);

        if(indicesFound->indices.size() > 0)
        {
            componentFound = Component(registeredCloud, indicesFound);
            sourceComponentsList.insert(iter.key(), componentFound); // use the same key used in the target list
        }
    }
}

int Pcqc::findTargetComponents()
{
    vector <PointIndices> clusters=findClusters(targetCloud,1,500,targetCloud->size());
    for(int i=0;i<clusters.size();i++)
    {
        PointIndices::Ptr currentIndices(new PointIndices);
        *currentIndices=clusters.at(i);
        int randomIndex = clusters.at(i).indices.at(rand()%clusters.at(i).indices.size());

        Component componentFound(targetCloud,currentIndices,randomIndex,1,255,1);//<------USA CLUSTER MATCH
//        Component componentFound(targetCloud,currentIndices,-1,1,255,1);   //<------USA SHAPE MATCH
        targetComponentsList.insert(QString("Component %1").arg(i),componentFound);
    }
    return clusters.size();
}

QMap<QString,bool> Pcqc::performChecks()
{
    QMap<QString,bool> results; // results list with checkname-result
    QMapIterator<QString,Check> checkIter(checksList); // iterator for the checks list
    QMap<QString, Eigen::Matrix4f> transformations;
    while (checkIter.hasNext())
    {
        Check currentCheck = checkIter.next().value(); // WARNING: with next(), we are moving the iterator one step forward
        QString componentName = currentCheck.getComponentName();
        cout <<componentName.toStdString()<<": ";
        Component sourceComponent;
        if(sourceComponentsList.find(componentName) != sourceComponentsList.end())
            sourceComponent = sourceComponentsList.find(componentName).value();
        Component targetComponent = targetComponentsList.find(componentName).value();
        CheckType type = currentCheck.getType();
        float threshold = currentCheck.getThresholdValue();
        if( transformations.count( currentCheck.getComponentName() ) == 0 && sourceComponent.getSize() > 0 )
//            transformations.insert(currentCheck.getComponentName(), currentCheck.getTransformation(&sourceComponent, &targetComponent));
            transformations.insert(currentCheck.getComponentName(), currentCheck.getTransformation(&sourceComponent, &targetComponent, sourceCloud)); // DEBUG della registrazione componente-componente sulla sourceCloud
        if(sourceComponent.getSize() > 0)
            results.insert( checkIter.key(), singleCheck(sourceComponent, targetComponent, transformations.value(currentCheck.getComponentName()), type, threshold) );
        else results.insert( checkIter.key(), false );
    }
    return results;
}

bool Pcqc::singleCheck(Component source, Component target, Eigen::Matrix4f transformation, CheckType type, float threshold)
{
    switch(type){
    case PRESENCE:
    {
        if (presenceCorrect(source, target, threshold))
            return true;
    }
        break;
    case POSITION:
    {
        if (positionCorrect(source, target, transformation, threshold))
            return true;
    }
        break;
    case INCLINATION:
    {
        if (inclinationCorrect(source, target, transformation, threshold))
            return true;
    }
        break;
    case ORIENTATION:
    {
        if (orientationCorrect(source, target, transformation, threshold))
            return true;
    }
        break;
    case COLOR:
    {
        if (colorCorrect(source, target, threshold))
            return true;
    }
        break;
    case HEIGHT:
    {
        if (heightCorrect(source, target ,threshold))
            return true;
    }
        break;
    case RADIUS:
    {
        if (radiusCorrect(source, target, threshold))
            return true;
    }
        break;
    }
    return false;//se non e' ritornato in uno dei casi con true, ritorna false
}

int Pcqc::segmentDifferences()
{
    cout<<"ScoreThreshold: "<<scoreThreshold<<" - DistanceThreshold: "<<distanceThreshold<<endl;
    int numberOfErrors = findWrongClusters(registeredCloud,targetCloud,differenceCloud,scoreThreshold,distanceThreshold);
    return numberOfErrors;
}

